/*******
** title:kalman filtering with c
** author:zhuchaoyang
** date:2014/10/31
*******/

/*本例采用简单的模型 x(k)=x(k-1)+w(k)
**                   z(k)=x(k)+v(k)
**kalman核心五步算法
**                   x(k|k-1)=x(k-1|k-1)
**                   p(k|k-1)=p(k-1|k-1)+Q
**                   x(k|k)=x(k|k-1)+k(k)(z(k)-x(k|k-1))
**                   k(k)=p(k|k-1)/(p(k|k-1)+R)
**                   p(k|k)=(1-k(k))p(k|k-1)
*/

#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <time.h>
#include <math.h>
/*
** 定义结构体
*/
/*typedef struct
{
    double** mat;
    int m,n;  //m与n分别表示矩阵的行与列
} matrix;
*/
#define rd (rand()/((double)RAND_MAX))
#define w (sqrt(Q)*randn())
#define v (sqrt(R)*randn())

double randn()
{
   // return rd+rd+rd+rd+rd+rd+rd+rd+rd+rd+rd+rd-6.0;
    return 2*((rand()/(double)RAND_MAX)-0.5);
}
void kalman()
{
    srand(time(0)); //随机种子
    float x_last=1;//状态初值x(k-1|k-1)
    float p_last=100;//p(k-1|k-1)
    float Q=0.1;
    float R=0.1;
    float k;      //增益
    float x_mid;  //x(k|k-1)
    float x_now;  //x(k|k)
    float p_mid;  //p(k|k-1)
    float p_now;  //p(k|k)
    int i;
    float x;//状态值
    float z;//测量值
    for(i=0;i<20;i++)
    {
        x = x_last+ w; //状态与测量方程
        z = x_last+ v;
            
        x_mid = x_last; //kalman五步核心算法
        p_mid = p_last;
        k = p_mid / (p_mid+R);
        x_now = x_mid+k*(z-x_mid);
        p_now = (1-k)*p_mid;
            
        p_last = p_now;  //更新p(k|k)值
        x_last = x_now;  //更新x(k|k)值

        printf("z position: %6.3f\n",z); //输出测量值
        printf("     x position: %6.3f\n",x); //输出状态     
        printf("kalman position: %6.3f  [diff:%0.3f]\n",x_last,fabs(x-x_last)); //输出状态滤波值
    }
}
void main()
{
    kalman();
}
